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ABSTRACT 

A sequential Extended Kalman Filter and Smoothing 
routine was developed to provide real time estimates of 
torpedo position and depth on the three dimensional under- 
water tracking range at the Naval Torpedo Station, Keyport, 
Washington . Inputs to the routine were acoustic pulse 
transit times from the target to receiving array elements 
which are non-linear functions of the position coordinates. 
These inpums were linearized and the filter gains and 
filtered estimates calculated on-line. By using a smoothing 
subroutine, all past filtered estimates were smoothed. 

Tests were conducted using simulated torpedo trajectories 
that traversed multiple hydrophone arrays. It was found 
that filter performance was dependent on system noise and 
the distance the torpedo was from the hydrophone array and 
the smoothed estimates of states were better than or equal 
to the filtered estimates. 
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INTRODUCTION 



I . 

The MUWES at Keyport, Washington currently operates two 
three-dimensional (3-0) underwater tracking range utilizing 
a sonar transmitter installed in the torpedo to be tracked. 
The transmitter is synchronized with a maszer clock. Timed 
acoustic pulses are received by bottom mounted hydrophone 
arrays and then relayed via cable to a computer at the 
observation site. The computer calculates the positional 
coordinates of the torpedo and plots its trajectory through 
the water . 

The measured data, which consists of the elapsed time 
from transmission of a pulse until its receipt at the 
hydrophone array, is corrupted with noise due to the 
combined effects of env i ronmental factors and measurement 
instruments . 

These noisy tracks are later analyzed, and measurements 
judged most inaccurate on the basis of total track statis- 
tics are removed in order to obtain a smooth representation 
of tne track. 

An opportunity exists for expanding the capability of 
the system by applying a real time Kalman Filter and post 
test Smoothing routine which can take as an input the 
transit times of the acoustic pulses, and produce the best 
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estimate of the position of the tracked object at a 
particular time. Previous research in this area C3l and 
[4], revealed that a Kalman filter utilizing a sequential 
estimation approach was desirable. 

The intention is to develop and test a sequential Kalman 
filter and smoothing algorithm that can be interfaced with 
the current underwater range system. 
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II. DESCRIPTION OF RANGE TRACKING GEOMETRY 



The hydrophone array, consisting of four independent 
elements, defines an orthogonal coordinate system in which 
transit time measurements are made. As shown in Figure 1, 
four hydrophones X, Y, Z, and C are on four adjacent 
vertices separated by a distance d, along the edge of the 
cube. The origin of the array coordinates is at the center 
of the cube with the orthogonal coordinates parallel to its 
edge. Positional information is computed from the transit 
times of a periodic synchronous acoustic signal travelling 
from the torpedo to the four hydrophones on the array. The 
range measures the tracked torpedo's position every 1 . 3 "1 
seconds to an accuracy that is typically within 3 to 30 
feet. A more detailed description of the range tracking 
capability is described in [2]. 
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Figure 1. 



Geometry of a Tracking Array 
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III. THEORY 



A. THE EXTENDED KALMAN FILTER 

Since the transit times were readily available and are 
nonlinear functions of position, these equations can be 
linearized and Kalman filter theory applied using the 
extended Kalman filter. This procedure produces a real-time 
system, filtering on the oransit times T^, T,^ , T,^ and T^, 
without the necessity of converting these times to 
positions . 

For the three-dimensional location problem three 
position states (x, y, z) and two velocity states 
specify target motion. The discrete linear and nonlinear 
observation equations are given by 



j<(k + ^) = • _x(k) + r ♦ w(k) 



( 3 . 1 ) 



and 



2(k) = hCx(k) , k) + v(k) (3.2) 

In these equations ^ and ^ are constant matrices and h is a 
nonlinear function of the state variable _x. w(k) is plant 
excitation noise and v(k) is measurement noise. The plant 
noise and measurement noise are assumed uncorrelated (white) 
with zero mean. That is. 
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£[w(k) • w'^(j)] = 

and 

E[v(k) • v'^(j)] = H(k)6i^;j 

with 

«kj = ^ = j 

= 0 , k ;£ J 

In order to apply the linear filter equation (3-2) 
expanded in a Taylor series about the best estimate of 
state at that time and only tne first-order terms are 
Equation (3*2) gives 

z ( k ) = H ( .-< ) • ^ ( k ) + _v ( k ) 

where 



X ' (k) = x(k/k-1 ) 

£(k/k-1) is a predicted value of the state before the 
measurement . 

A state error vector is defined by 
^(k/k) = x(k/k) - ^(k) , 

and a predicted state error vector is defined by 
x(k/k-1) = x(k/k-1) - x(k) . 



is 
the 
kept . 

(3.3) 

(3.3a) 

kth 
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The covariance of state error matrix is defined by 



P(k/k) = E[^(k/k) • £^(k/k)] , 

and the predicted covariance of state error matrix is given 
by 

P(k/k-1) = E[x(k/k-1) • x^(k/k-1)i . 



The state excitation matrix is given by 



Q(k) = r(k) E[w(k) • w^(k)] • r^(k) 



and the measurement noise covariance matrix is 



R ( k ) z E [ V ( k ) • V ( k ) ] 



The Kalman filter equations are given by [1]: 

P(k + l/k) = Jp(k/k)/ + Q(k) (3.4a) 
G(k) = ?(k/k-1 )H^(k) CH(k) -PCk/k-l )H^(k) + R(k)]’^ (3.4b) 
?(k) = [I - G(k)H(x)] ?(k/k-1) (3.^c) 
x(k+1/k) z ) x(k/k) (3.4d) 
z(k/k-l) z h(_x(k/k-1), k) ( 3 . 4 e) 
x(k) z x(k/k-1) + G(k)[z(k) - z(k/k-l)] (3-4f) 
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The Q matrix serves not only to allow for maneuvering 
but also to account for any model inaccuracies, that is, any 
discrepancies between the true action of the torpedo and its 
char acter i zation by Equation (3-1)- The Q also serves to 
prevent the gain matrix G(k) from approaching zero by always 
insuring uncertainty in the predicted covariance of error 
matrix P(k+1/k). 



3. OPTIMAL LINEAR SMOOTHING 

Smoothing is a non-real-time data processing scheme that 
uses all measurements between 0 and T to estimate the state 
of a system at certain time t, where 0 _< t <_ T . The 
smoothed estimate of _x(t) based on all the measurements 
between 0 and T is denoted by ^(t/T). 

Smoother error covariance is denoted by P(t/T) and 
P(t/T) _< P(t) means that the smoothed estimate of x{t) is 
always better than or equal to its filtered estimate. This 
is shown graphically in Figure 2. 

Several forms of the smoothing equations may be derived. 
One is the Rauch-Tung-Str i ebel form, which was used in our 
particular case with the discrete- time expressions 
summarized as follows [i]: 



x(k/N) = x_(k/k) + A.^[x(k+1/N) -x(k+1/k)] (3.5a) 




Figure 2. Advantage of Performing Optimal Smoothing [1] 
where 

-k = ?(k/k)i(k)^?(k + 1/k)“’’ for k = N-1 
?(k/N) = ?(k/k) + A^C?(k+1/N) - ?(k+1/k)]A,^^ ( 

also for k = N-1. 

In tnese equations _x(k/N) is smoothed State Estimate and 
^(k/N) is Error Covariance Matrix Propagation. 
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IV. PROBLEM DEFINITION - TORPEDO TRACKING WITH THE EXTENDED 
KALMAN FILTER AND OPTIMAL SMOOTHING 



A. FILTER EQUATIONS 

In the torpedo tracking problem, the non-linear 
observations are the four independent transit times from the 
tracked object to the hydrophones, T , T , T and T . Thus 

o A y ^ 

the non-linear measurement matrix z(k) is defined as: 



z ( k ) = 






Tx(k) 



T/k) 



T,C<) 



^Cx(k)+d/2)^-(y(k)+d/2)^+(z(k)+d/2)^] ^'^^^v(k) 
^[(x(k)-d/2)^+(y(k)+d/2)2+(z(k)+d/2)^] ^'^^+v(k) 

/ w L) 



X (k) 4.d/2 ) ^ + y ( k ) -d/2 ) ^ + ( z ( k) +d/2 ) ‘" ] ^ ( k ) 



[ ( X ( k ) +d/ 2 ) ( y ( k ) +d/ 2 ) ( z ( k ) -a/ 2 ) ^ ^ + v ( k ) 

(4.1) 



The measurement noises, v(k)'s, are assumed to be zero-mean 
and independent with a covariance matrix 



R(k) = 



0 

0 

0 



0 

0 



0 

0 



0 

0 

0 

2 



(4.2) 



17 



Equation (3.3a) can be used to give the linearized 
observation matrix. When the derivatives are taken and 
evaluated at the predicted state values x(k/k-1) = x'(k) the 
result is 



H(k) 



’VEL 



X’ (k)+d/2 
DENI 

X ' (k)-d/2 
X' (k)+d/2 



X ' (k ) +d/2 

DE?r^i 



0 

0 

0 

0 



y ' ( k)+d/2 

Dim 

y’ (k)+d/2 

— mrr~ 

y' (k)-d/2 

dZF3 

y ’ (k)+d/2 



0 

0 



2 ' ( k ) + d / 2 
— DTin — 

z ' ( k ) +d/2 
DUT2 

z ' (k)+d/2 
DEN 3 

z ’ (k)-d/2 
DEM 4 



(4.3) 



where 



DENI = [(x’(k)+d/2)^+(y’(k)+d/2)^+(z'(k)+d/2)^]^^- 
DEN2 = [(x' (k)-d/2)^+(y> (k)+d/2)-+(z' (k)+d/2)^]^^^ 
DEN3 = [(x’(k)+d/2)- + (y’(k)-d/2)- + (z’(k)vd/2)^] 



DEN4 = [(x ' (k)+d/2)-+(y ' (k)+d/2)^+(z ' (k) -d/2)^] ^ 



The torpedo dynamics used far the tracking problem are 

2 

assumed to be I/s'" with estimations on five states x 
position, X velocity, y position, y velocity and z position 
(height of torpedo above hydrophone array) . 

The means of the random excitation and random noise are 
assumed to be zero, i.e., 
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£Cw(k)] = 0 
E[v(k)] = 0 



Four measurements are taken every 1.31 seconds, 

2 

one time slot, and with this sampling time the 1/s 
has state transition, (PHI) and gamma, (") matrices 
to : 



1 



TOO 



$ 



0 10 0 
0 0 1 T 

0 0 0 1 



0 0 0 0 



0 

0 

0 

0 

1 



and 



t2/2 



i 

0 



0 

0 

T“/2 



0 

0 

0 



0 T 0 

0 0 T 



The ^ matrix, Q matrix, R matrix, and H matrix 
used in the Kalman filter equations (3-^)* 



which is 

plant 

equal 



(4.4) 



(4.5) 



re then 
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B. THE SEQUENTIAL EXTENDED KALMAN FILTER 



In the sequential approach, the basic Kalman filter 
equations (3-4) must be modified. Calculations are 
performed on each of the four independent transit times in 
the following order; T , T , T and T for each 1.31 second 
time slot. The estimate of the states x(k/k), based on one 
transit time measurement are used as the prediction x(k/k-l) 
for the calculations on the next measurement. Thus for the 
first time measurement T^ only the first row of the 
linearizing H matrix is calculated. 

Next the first gain column corresponding to the first 
time measurement T^ is calculated by 



P(k/k-1) H: 



^icol 



irow 






(4.6) 



where i = 1 to 4 corresponding to the four measured transit 
times. Thus, the first row of the H matrix is used to 
calculate tne first column of the gain matrix with both 
corresponding to the first measured time T^. 

Next, an estimate of the particular observation time is 
calculated using equation (3.4f) evaluated at the predicted 
state _x ( k/k- 1 ) . 

The difference between observed transit time and the 
estimated transit times forms the residual which is used in 
the estimate equation 
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= x(k/k-1) + [Residual] 



(4.7) 



This equation gives an estimate of the states based on one 
of the four time measurements. 

Next, covariance of error is calculated based on one 
measurement by 



= FT _ 



^icol 



.row 



h-i 



where 



(4.3) 



I = identity matrix 

P^. ^ = the covariance .matrix calculated from the 

previous transit time measurement or if i = 1 , 
the prediction P(k/k-'l). 



After the first iteration, x.| becomes £(k/k-1) and P^ 
becomes P(k/k-l) for the second iteration which calculates 
the estimate of the states based on the second measurement 

h- 

After four iterations (k = 4), becomes the esti.mate 
for the time slot x(k/k) and ?„ becomes the covariance error 
P(k/k) . 

The predictions for the next time slot are calculated 
using equations (3.4a) and ( 3 • 4d ) . This process is repeated 
for each time slot . 
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C. OPTIMAL SMOOTHING PROCESS 



During the running of the Extended Kalman filter and 
Smoothing routine, after the forward filter pass for each 
time slot (except the first), the smoothing subroutine is 
called. By using the present and previous filtered estimate 
of _x(t), a smoothed estimate of previous is calculated. 

This process is repeated for each past time slot. 

Solution of the equations (3-5) proceeds as follows: As 

an example, and because it is slightly easier to see when 
actual times are used, suppose N =30- On the forward 
filter pass, the values £(k/k) , ^(k/k-1), £(k/k), and 
_P(k/k-1 ) would be computed and scored. On the final 
iteration of the forward pass, with k = N = 30, 

x(30/30) = x(30/29) + G(30) [z(30) - H x(30/29)J 

i.e., we have computed and stored £(30/30). 

Now, the smoothing process starts in the reverse 
direction. Decrement k to k = N - 1 =29, then 

x(29/30) = £(29/29) + A(29) [£(30/30) - £(30/29)1 
stored stored stored 

and A(29) = P(29/29) ^ ?(30/29)‘^ 

stored stored 
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Let k = M - 2 = 28, then 

x(28/30) = x(28/28) + A(28) Cx(29/30) - x(29/28)] 

stored computed stored 

last 

iteration 

and A(28) = P(2S/28) P(29/28)'^ 

stored stored 

Also, for each of the two preceding iterations. 



P(29/30) = PC29/29) 


+ A(29)l?(30/30) 


- ?(30/29)] A^(29) 


stored 


computed 


stored 


stored computed 


P(28/30) = P(28/28) 


+ A(28):?(29/30) 


- PC29/28)] A^(28) 


stored 


computed 


stored 


stored computed 

» 
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V. TESTING AND SIMULATION 



A. DESCRIPTION 

The sequential Extended Kalman Filter and Smoothing 
routine is tested using simulated torpedo tracks. A variety 
of track scenarios were produced to test the filter and 
smoothing performance during single and multiple arrays 
tracking . 

Computer generated tracks were tested in the first 
series of straight running, constant depth and constant 
velocity torpedoes. A variety of track scenarios were used 
transiting through multiple quadrants including; 

1. Crossing north of the array. 

2. Crossing diagonally through the array. 

The next series of tests demonstrates the ability of the 
filter to track through the areas of multiple arrays 
including : 

1. Crossing above the arrays. 

2. Crossing diagonally through the arrays. 

All runs were made with a variety of initialization 
errors in position and velocity. 

Zero mean Gaussian noise is added to corrupt the 
observed transit times for all runs. 




% 




3. 



THE GATING SCHEME 



The operation of the filter may be adversely affected by 
large measurement noise. One error of a relatively large 
magnitude could invalidate the filtered output for many 
subsequent time slots. Before random measurement noise and 
random excitations could be added to the observed times for 
testing, a form of protection was designed to guard against 
catastrophic failure. This protection is provided by 
establishing limits of acceptability for each of the 
measurements . 

Measurement errors can occur because of many factors 
including an error in the transit time of the acoustic pulse 
primarily due to the receipt of multipath signals that have 
bounced off the surface, bottom or different density layers. 

A three-sigma gate was designed using the covariance of 
measurement noise (R) and the covariance of estimation error 
(P(k/k) ) . 

For each calculation of a state estimate (x(k/k)), the 
largest positional covariance of error was used, either x, y 
or z, and converted to time in seconds using the average 
velocity of sound in water for Dabob Bay, 4360 ft/sec. The 
gate then was written for each time measurement i = 1 to 4: 



GATE 




The gate expands or decreases depending on the confidence 
level of the position estimate and the transit time. If 
ZDIFF, which is the difference between the actual transit 
time received and the predicted transit time to a particular 
hydrophone, exceeds the gate, the measurement is considered 
unacceptable and the filter gain is set to zero causing the 
filter to ignore the data and take the prediction of the 
states as the estimate 

x(k/k) = ^(k/k-1 ) 

An invalid time measurement zeros only the gain column for 
that particular hydrophone causing only that hydrophone's 
data to be ignored . 

C. MULTIPLE ARRAY TR.ACKING 

Initial tests viere performed on tracks in the area of 
one array. In order to more closely simulate a typical run 
on the range, a scheme was designed to track the torpedo 
through multiple arrays. 

First, a coordinate system is defined as shown in 
Figure 3* The center of the coordinate system is geographi- 
cally near the entrance to Dabob Bay in che simulation. 

Array number 6 is the closest array to be coordinate center. 
In the simulation array 1 is at 36,000 feet from coordinate 
center and array 6 is 6,000 feet. The C hydrophone is 
assumed to be the axis location of each array. Then each X 
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iTfORO — Hydrophone Location Matrix 



Figure 3 
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position for the X hydrophone in each array is + 30 , each 
Y position for the Y hydrophone is Y^ + 30, and each Z 
position for the Z hydrophone is Z^ + 30. These 72 
positions, an XYZ position for each of 4 hydrophones in 6 
arrays, were placed into a 6 x 12 matrix HYDRO and 
referenced throughout the routine. 

The geometry centered on each array is taken out of the 
problem and the target position is based on a central 
ref erence . 

The non-linear time equation becomes 

T = 1/VEL,y^(x - Xq)^ + (y - y + (z - 

where x^ , y^ , or z^ is the position of a particular hydro- 
phone and array being used. 

The decision parameter used to determine the switching 
from array to array is a straight, handoff. If the predicted 
X position is greater than 3»000 feet from the array in use, 
then an index (18) is incremented and the next row of HYDRO 
is implemented. This placed into the routine the x, y and 2 
positions of the hydrophones in the next array. The handoff 
can easily be utilized in real range operations, as the 
transit times from adjacent arrays are present at the 
computer for a particular time slot. For simulation, it is 
assumed that in all the arrays each axis pointed in the same 
direction. In actual range operations each array is tilted 
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about both the X and Y axis. Since the true transit times 



are derived in a tilted coordinate frame, the filter's 
estimate of transit time must also be calculated in a tilted 
coordinate frame. The tilt angle measurements along with 
the level rectangular coordinates of the array with respect 
to the central rectangular coordinate system can be input 
into the matrix HYDRO to rotate the coordinates of each 
hydrophone in the array. 
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VI. TEST RESULTS 



A. SERIES ONE 

Figure 5 shows the true trajectory of the torpedo in the 
horizontal X-Y plane during a straight run through single 
array. Torpedo velocity is 50 knots in the x-direction. 
Initial position errors are set to 25 feet for X and Y. 
Velocity errors are set to zero. Figures 6, 7 and 3 depict 
the position errors for both Kalman filter and Smoothing. 
Measurement noise is added to all runs. The steady state X 
and Y position errors ranged between -6 and +9 feet through- 
out the trajectory for Kalman filter and -2 and +4 feet for 
smoothing. The position errors are computed by subtracting 
the filter position estimate, x(k/k), for Kalman filter and 
x(k/N), for smoothing, from the computer generated true 
position for each time slot. Figures 9, 10, 11, 12, and 13 

depict the mean square estimation errors of states. These 
estimation errors are obtained by taking the appropriate 
diagonal terms of the covariance matrix and smoothed 
covariance matrix. 

B. SERIES TWO 

Figure 14 shows the true trajectory of the torpedo in 
the horizontal X-Y plane, during a crossing run through 
single array. Torpedo velocity is 40 knots in X-direction 
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and 25 knocs in Y-direction . The torpedo depth is 
maintained at 300 feet. Figures 15, 16, and 17 depict the 

position errors. Since the initial position errors are set 
to zero, the position errors ranged between -3 and feet. 
Figures 18, 19, 20, 21, and 22 depict the mean square 
estimation errors of states. 

C. SERIES THREE 

Figure 23 shows the true trajectory of the torpedo in 
the horizontal X-Y plane, during a straight run through 
multiple array. Because of storage problem of the computer, 
the runs through the multiple array were made for 190 time 
slots. Torpedo velocity is 50 knots in X-direction and the 
depth is 100 feet. Figures 24, 25, and 26 show the position 
errors ranged between -6 and 17 feet. Figures 27, 28, 29, 

30, and 31 depict zhe mean square estimation errors of 
states. Figure 32 shows the error ellipsoids superimposed 
at every eighteenth observation. The error ellipsoids are 
expanded to twenty-five times their true value in order that 
they may be seen. The error ellipsoids provide a geometric 
interpretation of the behavior of she estimator. Before the 
hand-off point, at the ninetyth time slot, the major axis 
rotation of the error ellipsoid and magnitude of the axis 
were -16.98 degrees and 43.23 feet, respectively. After the 
hand-off point major axis rotation became 25.8 degrees, and 
its magnitude became 1.6 feet. When the magnitude of an 



axis of the ellipsoid decreases, the conclusion is that the 
error in the estimate decreases, because the observation 
from the new array has an error covariance ellipse rotated 
over 40 degrees from the present covariance (-16.94 degree) 
of the estimate. The ellipsoids are extremely narrow. When 
combined the resultant covariance is reduced greatly. 

Figure 4 depicts the result of reduction and rotation of the 
ellipsoids. Figure 33 shows the error ellipsoids before and 
after the hand-off point. 




figure 4. Result of Rotation and Reduction of the 
Error Ellipsoids 
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D. SERIES FOUR 



Figure 3^ shows the true trajectory of the torpedo in 
the horizontal X-Y plane, during a crossing run through 
multiple array. Torpedo velocity is 50 knots in X-direction 
and 40 knots in Y-direction. The torpedo depth is main- 
tained at 300 feet. Initial position errors are set to 25 
feet for X and Y and initial velocity errors are set to 5 
knots. Figures 35, 26, and 37 show the position and depth 
errors. Since the initial position and velocity errors are 
set to 25 feet and 5 knoos , the big position errors were 
taken at the beginning of the run. These values were 
ignored from the figures in order to see clearly to the rest 
of the run. Figures 33, 39, 40, 41, and 42 show the mean 
square estimation errors of states for both filtered and 
smoothed . 
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VII. CONCLUSIONS 



The sequential Extended Xalman Filter and Smoothing 
satisfactorily provided real time estimates of torpedo 
position and depth. The average of steady state position 
and depth errors ranged between 3 and 1 feet for torpedo 
tracks within the specified radial tracking range after 
Kalman filter. These errors had a range of around 1 foot 
after smoothing. 

The filter performance was dependent on system noise and 
the distance the torpedo was from the hydrophone array and 
the smooched estimates of states were better than or equal 
to the filter estimates. 

Implementation at the range computer facilities can be 
accomplished by real time Kalman Filtering and post run 
Smoothing of the raw time data. Future tests should include 
evaluating filter performance using trajectories generated 
from actual torpedo runs on the Dabob test range. These 
tests would verify the adequacy of the noise model in the 
filter and the ability of the software to edit erroneous 
transit time measurements. 

The rotation and reduction of the error ellipsoids 
(i.e., the filter error covariance) was most instructive and 
gave much insight into the performance of the filter. 
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Figure 5. True Trajectory of the Torpedo During a Straight Run Through 
Single Array (Right to Left) 
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Figiare 6. Error in Torpedo X-Fosition During a Straight Run Through 
Single Array 
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Figure 7„ Error in Torpedo Y-Position During a Straight Run Through 
Single Array 
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Figure 8. Error in Torpedo Z-Position During a Straight Run Through 
Single Array 
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Figure 9, Mean Square Estimation Error (ft„ ) in X-Position During 
Straight Run Through Single Array 
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Figure 11, Mean Square Estimation Error (ft,“) in Y-Position During 
a Straight Run Through Single Array 
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Figure 12. Mean Square Estimation Error (ft. /sec, ) in Y-Velocity 
During a Straight Run Through Single Array 
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Figure 13. Mean Square EyCimation Error (ft. ) in Z-Position During 
Straight Run Through Single Array 
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-Position (ft.) Results of Straight Run Through Single Array 
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Figure 14, True Trajectory of the Torpedo During a Straig 
Run Through Single Array 
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Figure 15. Error in Torpedo X-Position During a Straight Run Through Single 
Array 
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Figure 16., Error in Torpedo Y-Position During a Straight Run Through Single 
Array 
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Figure 17 „ Error in Torpedo Z-Position During a Straight Run Through Single 
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Figure 18. Mean Square Kstiination Error (ft. ) in X-Position During 
Straight Run Through Single Array 
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Figure 19. Mean Square Estimation Error (ft, /sec. ) in X-Velocity During 
a Straight Run Through Single Array 
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Figure 20 * Mean Square Estlitiation Error (fto ) in Y-Position During 
Straight Run Through Single Array 
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Figure 21. Mean Square Estimation Error (ft. /sec, ) in Y-Velocity During 
a Straight Run Through Single Array 
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Figure 22. Mean Square Estimation Error (ft, ) in Z-Position During 
Straight Run Through Single Array 
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Figure 23. True Trajectory of the Torpedo During a Straight Run Through 
Multiple Array 
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Figure 24. Erx'or in Torpedo X-Position During a Straight Run Through 
Multiple Array 
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Figure 25. Error in Torpedo Y-Position During a Straight Run Through 
Multiple Array 
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Figure 26. Error in Torpedo Z-Position During a Straight Run Through 
Multiple Array 
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Figure 27 „ Mean Square Estimation Error (ft. ) in X-Position During 
Straight Run Through Multiple Array 
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Figure 28. Mean Square Estimation Error (ft. /sec. ) in X-Velocity During 
a Straight Run Through Multiple Array 
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Figure 29, Mean Square Estimation Error (ft, ) in Y-Position During 
Straight Run Through Multiple Array 




UT O 

ou rvj -- 



o 






O 



ul 

o 

_J 

CO 



1 2 A “ A 



dO dOddd NOIiUW 



b d d d d I i U ‘3 N b d N 



63 



Figure 30. Mean Square Estimation Error (ft. /sec. ) in Y-Velocity During 
a Straight Run Through Multiple Array 




6 ^ 



Figure 31, Mean Square Estimation Error (ft, ) in Z-Position During 
Straight Run Through Multiple Array 
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Figure 33. Error Ellipsoids Before and After the Hand-off Point During 
a Straight Run Through Multiple Array 
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Figure 34. True Trajectory of the Torpedo During a 
Straight Run Through Multiple Arrays 
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After Kalman Filter 
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Figure 35 « Errors in Torpedo X-Position During a Straight Run Through 
Multiple Arrays 
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Figure 36 „ Errors in Torpedo Y-Position During a Straight Run Through 
Multiple Arrays 
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Figure 37. Errors in Torpedo Z-Position During a Straight Run Through 
Multiple Arrays 
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Figure 38. Mean Square Estimation Error (ft. ) in X-Position During 
a Straight Run Through Multiple Arrays 
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Figure 40. Mean Square Estimation Error (ft^ ) in Y-Position During 
a Straight Run Through Multiple Arrays 
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Figure 42. Mean Square Estimation Error (ft.^) in Z-Position During 
a Straight Run Through Multiple Arrays 



APPENDIX A 



PROGRAM DESCRIPTION AND FEATURES 

The sequential Extended Kalman Filter and Smoothing 
routine used for torpedo tracking is modularized for ease of 
implementation. The program is general in nature and many 
of the parameters of the filter are variable including: 

a. The number of states in the filter -- M 

b. The number of random forcing functions -- M 

c. The number of measurements -- JS 

d. Number of time slots — JTIME 

The constant matrices PHI, R, COVW, and GAMMA are 
initialized in the beginning of the program using data 
statements. The filter is initialized with ?(1/0) and 
x(1/0) (initial covariance of estimation error and states) 
using subroutine INIT. The first estimate is at time 1 and 
continues until ITIME = JTIME + 1 . True measurement times 
(ZI) are computed using either subroutines TRAJEC or TRAJC3, 
depending on whether single array or multiple array tracking 
is implemented. Either subroutine will compute four 
measurement times (T^, T^, T^, T^ ) for each time slot. The 
measurement times are corrupted by zero-mean, white Gaussian 
noise using the I3M-3033 subroutine GGNML. For each of the 
four time measurements the corresponding row of the 
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linearizing H matrix is calculated using either subroutine 
CHROW or CHR0W3 , depending on whether single array or 
multiple array tracking is used. The corresponding gain 
matrix column GI is then found. These row and column values 
are utilized in forming the covariance of estimation error. 
PI, for that particular time measurement. Next the estimate 
of the observation time ZHAT from that particular hydrophone 
is formed using the subroutine CZHAT or CZHAT3, depending on 
whether single or multiple array tracking is implemented. 

The residual ZDIFF(I) = ZIC(I) - ZHAT, is then calculated. 
Finally the estimate of the states XI based on one time 
measurement is calculated and the process is repeated for 
the next measurement. After four iterations, XI becomes the 
state estimate XXK and PI becomes the updated covariance of 
estimation error PKX, and the predictions of the states and 
covariances XXKMT and PXXM1 are formed. Finally, for each 
time slot (except the first) smoothed state estimates, XXXS, 
and covariances, PKXS, are formed using the subroutine 
SMOOTH. PLOT? is used to generate line printer plots and 
PLOTG is used to generate VFRSATSC plots. 

A. PROGR.^^M SUBROUTINES 

A brief description of the subroutines are described 
below : 

1. TRAJEC -- This subroutine develops the torpedo 
trajectory which is used as truth data for zhe filter. The 
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subroutine outputs true transit times, ZI(I), and the x, y, 
z positions, TD(I), of the torpedo for each time slot. THe 
subroutine is used during single array tracking. 

2. TRAJC3 — This subroutine performs the same function 
as TRAJEC but is used only during multiple array tracking. 

3 . INIT — This subroutine generates the initial state 
vector x(0/-1) and initial covariance matrix P(0/-1). 

4. CHROW -- This subroutine computes the appropriate 
row of the linearizing H matrix. Each row corresponds to 
one of the four transit time measurements, T^ , T^, T^ , T^ . 
This subroutine is used during single array tracking. 

5 . CHR0W3 — This subroutine performs the same 
functions as CHROW but is used only during multiple array 
tracking . 

6. CZHAT -- This subroutine computes the estimated 
transit times for the filter. Four transit cimes, ZHAT, are 
calculated corresponding to each of the four true transit 
times ZI(I). This subroutine is used during single array 
tracking . 

7 . CZHAT 3 — Subroutine performs same functions as 
CZHAT however it is used only during multiple array 
tracking . 

8. QFIMD — This subroutine develops an adaptive Q 
matrix which is a function of the torpedo velocity. Three 
input variables defined in a data statement at the beginning 
of the program can be adjusted: 
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aa . SIGACC -- Maximum expected horizontal 
acceleration of the torpedo. 

bb . SIGDIV — Maximum expected change in vertical 
velocity . 

cc . SIGCC -- Maximum expected turn rate of the 
torpedo in the horizontal plane. 

The values listed in the program were used and kept 
constant during the simulation tests. If the user desires 
not to use the adaptive Q subroutine, software code is 
provided at the beginning of the program to calculate a 
constant Q matrix. 

9. GGNML — This is an I3M-3033 subroutine contained in 
the IMSL library. The routine generates zero mean white 
Gaussian noise with an RMS value normalized to 1 . The main 
program scales the noise and adds it to the transit time 
measurements . 

10. PLOTP — This is an I3M-3033 subroutine used to 
generate the line printer plots. Information on this 
subroutine can be obtained from the IMSL library. 

11. PLOTG — This is an I3M-3033 subroutine used to 
generate the VERSATSC plots. Information on this subroutine 
can be obtained from the IMSL library. 

12. SMOOTH -- This subroutine computes the smoothed 
state estimates and covariances. 
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B. UTILITY PROGRAMS 



These subroutines were designed to be 
repetitive matrix and vector manipulations 



1 . 


PROD - 


- multiplying two matrices 


2. 


MMULT 


— multiplying a matrix and 


3. 


VMULT 


— multiplying two vectors 


4. 


TRANS 


— transposing a matrix 


5. 


ADD — 


adding two matrices 


6. 


SUB — 


subtracting two matrices 


7. 


RECIP 


— inversing a matrix 



used for 



a vector 
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APPENDIX B 



SEQUENTIAL EXTENDED KALMAN FILTER AND 
SMOOTHING PROGRAM LISTING 
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DATR( 1 ) = DATR U )+DATR( I+3)*DATR(14) +({ ( DATR( 14) )**2 ) /2 ) *DATR ( I +6 } 
CONTINUF 

lF(ABS(T-it31) .LE. 0.0001) RETURN 

GO TO 13 

END 
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